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I. INTRODUCTION 



In any operational scenario of an underwater vehicle there exits a triple-nested 
sequence of mission accomplishment operations: Path planning, navigation, guidance, and 
autopilot design. The path planner takes information from charted obstacles and friendly 
or hostile environments and generates a smooth plan for the vehicle to follow. A certain 
level of feedback exists in this operation through the use of sonar beams in order to 
replan a path when uncharted objects are encountered or when the mission requirements 
have changed. Based on the desired vehicle positions and orientations at certain points, 
several classes of smooth paths containing sets of straight line segments, and circular arcs 
or cubic splines can be obtained [1]. Once a smooth path is generated, the navigator 
provides through a selected guidance law the appropriate vehicle heading commands 
which are in turn delivered by the autopilot. Line of sight guidance using a discrete series 
of way points was studied by Lienard in [2] using sliding mode heaving, propulsion, and 
depth keeping autopilots. The scheme demonstrated excellent stability and robustness 
characteristics, although the actual vehicle response was found to lag significantly the 
commanded straight line paths. The guidance and autopilot functions can be combined 
when the lateral deviation off the desired path is directly incorporated into the control law 
design. This leads to the development of a cross track error autopilot. Such schemes have 
been studied by Chism [3] and Hawkinson [4] for the single input and multiple input 
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case, respectively. Cross track error autopilots provide more accurate path keeping 
response but they must be designed more carefully since they tend to be more dependent 
than heading controllers on an accurate description of the vehicle hydrodynamic 
characteristics. The main reason for this is the increase in the system dimensionality by 
one. Underwater vehicles ojjerate in changing environments over a wide range of 
operating speeds and, therefore, a certain degree of uncertainty exists in the vehicle 
dynamic modeling. Cross track error autopilots also require accurate positional 
information updates at the same rate as heading and heading rate. 

For these reasons, in this work we go back to the case of a heading autopilot 
coupled with an orientation guidance law. The two main tasks on which we will 
concentrate are as follows: First, we must develop a way of establishing stability of the 
combined autopilot/guidance scheme for straight line commanded motions. Second, the 
actual vehicle response characteristics must be made to resemble the desired cross track 
error response with smooth transitions between consecutive straight line segment and with 
minimal path overshoot. A linear full state feedback control law is used to adjust the 
heading of the vehicle to any desired value, and a pure pursuit guidance law is used to 
provide the commanded heading for straight line motion. In this scheme the vehicle 
commanded heading equals the line of sight angle between the vehicle center and a target 
point moving on the desired path at a constant lookahead distance from the vehicle. This 
parallels the case studied in [5] except that in our case the existence of lateral and 
rotational dynamics add more complications to the problem. All computations aie 
perfonned for the Swimmer Delivery Vehicle [6] for which a set of hydrodynamic 
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characteristics and geometric properties is available. Problem formulation and equations 
of motion are presented in Section II. The analysis procedure is outlined in Section IE, 
and simulation results are presented in the Section IV. Finally, conclusions and 
recommendations for further research are given in Section V. 
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II. DEVELOPMENT OF THE MATHEMATICAL MODEL 



A. EQUATIONS OF MOTION 

In a moving coordinate frame fixed at the vehicle center, Newton’s equations of 
motion for a rigid body in the horizontal plane are 

m(\> +ur +x^ -y^r^) = Y , (2.1) 

+nix^{v +ur) -my^vr = N , (2.2) 



where 

V = sway(lateral) velocity, 
r = yaw(angular) velocity, 
u = forward(surge) speed, 

Y = sway force, 

N = yaw moment, 
m = vehicle mass, 

= vehicle mass moment of inertia, 

(xQ,yo) = coordinates of center of gravity. 

Expanding the force Y and moment N in added mass, dampuig. and drag tenns. 
equations (2.1) and (2.2) are written as 
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m{\> + ur +AgT -y r^) - 2-1 *Yr + 2.1 + Yur) 



■ £/ ^Yuv - ^jCphix)2212!l-dx + 2l \u 25 , 



(2.3) 



V +xr 



+ mxj^y + ur) - my^vr = 2.1 ^Nr + 2l ^{N.v +Nur) 



2l ^Nuv - 2jc^hix)y!222l-xdx + 2l \u % 



(2.4) 



V +xr 



where 

p = water density, 

1 = vehicle length, 

S = rudder angle, 

h(x) = vehicle height distribution, 

Cdj, = drag coefficient. 

The inertial position of the vehicle (x,y) and its heading angle xjt ( see Figure 1 ) 
are given by 



Y = r , 



(2.5) 



X - McosY~vsinY , 



( 2 . 6 ) 



y = MsinY+vcosY ■ 



(2.7) 
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Figure 1 Top view of the vehicle 
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B. STATE SPACE EQUATIONS 

Choosing ( Y. r ) as the state vector, the linearized state space equations (2.3), 
(2.4), and (2.5) are written as 



Y = r , 



( 2 . 8 ) 



V = a^^uv+a^^ur+b^u^^ , 



(2.9) 



r = a^jUv+a^^ur+b^u^S . 



( 2 . 10 ) 



The coefficients in equations (8), (9), and (10) are given by 



(/-0.5p/'N,)(0.5p/^y„)-(mX^-0.5p/^>V)(0.5p/X.) 

(/^-0.5p/’M)(m-0.5p/^v,)-(/tiXc-0.5p/^y,)(mX^-0.5p/X) 

(/-0.5p/5N,)(-m+0.5p/^y^)-(mX^-0.5p/V(-mX^+0.5p/X) 

(/-0.5p/’N,)(m-0.5p/^y.)H»iX^-0.5p/V,)(mX^-0.5p/X) 

(»iX^ -0.5p/ ^y,)(0.5p/ ^NJ) -(mX^ -0.5p/ ^N,)( -m +0.5p/ 
(/-0.5p/’N,)(m-0.5p/^y,)-(mX^-0.5p/^y,)(mX^-0.5p/^A^.) 

(mXg -0.5p/ \)i -mX^ +0.5p/ “N ) "0-5p/ XX 

(/ -0.5p/ ^N,)(m -0.5p/ ^y,) -{mX^ -0.5p/ ^y,)(mX^ -0.5p/ ^.,) 

(/-0.5p/^N,)(0.5p/^y5)-(mX^-0.5p/>,)(0.5p/W5) 

(/ -0.5p/ ^N.)(wi -0.5p/ \.)-(mXc-0.5p/ % .)(/?iX^-0.5p/ ^N.) 



1 



b. 



(mX^-0.5pl%)i0.5pnN,)-imX^-0.5pl%X0.5pl^y^ 
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(/ -0.5p/ %){m -0.5pl -(mX^-0.5p/ ^3^.)(mX^-0.5p/ %) 



where 

Jg’ ^'r ^r’ “ lateral hydrodynamic coefficients 

N^, N^, N^, N., N^, = yaw hydrodynamic coefficients 

Equations (2.8), (2.9), and (2.10) describe the dynamics of the system with respect 
to small deviations around a nominal direction Y = 0 • 

C. PATH KEEPING DEVELOPMENT 
1. Heading control 

A linear full state feedback control law is of the form 



5 = kp^+k^v+k/ 



( 2 . 11 ) 



where k„ kj and kj are the three gains. 



From equation (2. 8), (2.9), (2. 10) and (2.11), the closed loop system is 



Y = r 



( 2 . 12 ) 



r' 



b^u ^k^y\^+{a^^u+b^u ^k^)v+{a^^u ^b^u ^k^)r 



(2.13) 
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r = b^u'^k^^+{a^^u+b^u^kj)v+{_aj^u+b^u^k^)r 



(2.14) 



The characteristic equation of (2.12), (2.13) and (2.14) is 



0-X 


0 


1 






b,u\ 


a^^u+b^u^k^-X 


a^^u^b^u% 


- 


0 


b^u% 


a^^u+b^u ^k^ 


a^jU+b^u ^k^-X 







-b^u ^k^{a^^u-^b^u ^k^)+b^u '^k^{a^^u+b^u ^k^-X) 



%\a^^a^^u^^a^^b^u ^k^-Xa^^u+b^a^^u ^k^+b^b^u ^kJc^-Xb^u ^k^-Xa^^u 

-Xb^u ‘^k^-^y}-a^^a^^u'^-a^^b^u ^k^-a^^b^u ^k^-bfi^u *k^k^ 

-bfi^^u ^kj -bp^u *kp^+a^p^u ^k^ ^bp^u ^kp^-Xb^u ^k^ 



X^-{a^^u+b^u ^k^+a^^u+b^u ‘^kpX^+{a^^a^^u'^+a^pju ^k^+b^a^^u ^k^ 
-a^^a^^u^-a^p^u ^k^-a^p^u ^k^-b^u ^k^)X+b^a^^u^k^-b^a^^u ^k^ 



2. Desired characteristic equation 

The 3rd order ITAE polynomial is defined by 



X^+1.75(Oo>.2+2.15(Oo>^+cOo = 0 



0 



0 



0 (2.15) 



(2.16) 
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©0 = 10 — 



where 

= settling time (dimensionless) 
From equations (2.15) and (12.6) we get 



-a^^u-b^u'^k^-a^^u-b^u'^k^ - 1.75©^, 



(2.17) 



-a^^b^u^k^-a^p^u^k^-b^u'^k^ = 2.15©o 



(2.18) 



b ^ a ^ yk ^- b ^ a ^^ u ^ k ^ = ©„ 



(2.19) 



The system of equations (2.17), (2.18) and (2.19) can be solved for the three gains 



A'. = 



©r, 



Vn“'-V 2 i«' 



K = 



u\-\n5(a^-a^^-a^){a^p^-a^^b^)-b^u\2A5(i^-a-na^u^+a^^a^^u^+b^u ^A,) 
b,« \ a ^ p ^- a ^^ b ^)- b^u \ a ^ b ^ -^,2^2) 



A, = 



b2«^[2.15©o-M ^(«,,«22”^I2^2i)'*'V ^("1 .75©o-a,,M -a22«)(^22^I "«12^2) 

b,M \a^p^-a^^b^)-b^u \a^b^ 
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3. Pure pursuit navigation 



For a pursuit navigation 
= a 

where 

\)/e = commanded heading 

Referring to Figure 1, the line of sight angle a is defined by 
CJ = -tan"'-^ 

where is the vehicle lookahead distance, and the control law (2.11) becomes 

6 = 

Using equations (2.20), (2.21) and (2.22) we get 

8 = k^{y^f+tan''—)+k^v+k^r 

The linearized equation (2.23) becomes 
5 = k^{\^+—)+k2V+k/ 

^d 

The linearized equation for the lateral deviation y is obtained from (2.7) as 

y = «Y+v 



( 2 . 20 ) 



( 2 . 21 ) 



( 2 . 22 ) 



(2.23) 



(2.24) 



(2.25) 
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Now the complete state vector is y, v, r and y , and the state space equations are written 
as 



V 

V 



r 



h^u'^k^ +(a , , 1/ +6 , « V +(a +6 , M J_y 



b^u ^k^)v +(^ 22 “ "^^ 2 “ ^^ 3 )^ "^^ 2 “ ^* 1 — 

^ j 



y = i/y+v 



and the characteristic equation is 



( 2 . 26 ) 



( 2 . 27 ) 



( 2 . 28 ) 



( 2 . 29 ) 



0-X 


0 


1 


0 


b^u^k^ 


a^^u+b^u'^k^-X 


a,2« +b^u ^k^ 


b^u^k^— 


b^u ^k^ 


a^^u+b^u ^k^ 


^ 22 “ ■*■^ 2 *^ ^^3 


b^u ^k^— 


u 


1 


0 


0-X 



( 2 . 30 ) 
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m. COMPUTER SIMULATION 



A. PROGRAMMING 

1. Program in MATRIX.X 

The MATRIX.X software is in VAX/VMS in the Mechanical Engineering 
Department. The program in MATRIX.X is written to find gains and poles of the system 
from the given inputs, forward speed (u), settling time (t^) and vehicle lookahead distance 
(Xj). This is used to compute the eigenvalues of the complete system ( Equation (2.30). 

The four eigenvalues of the system are computed for the given values of u, t^. and x^. 
For stable vehicle response, all four must be negative (or have negative real parts). If at 
least one is positive, the vehicle response wUl be unstable and convergence to the straight 
line path is not to be expected. In such a case, the parameters (in particular the lookahead 
distance must be changed) so that the vehicle is stable. A listing of this program is 
presented in Appendix A. 

2. Program in FORTRAN 

The first program in FORTRAN, is written to find distance along body fixed 
axis ( x-y ) from inputs, heading angle (Y), perpendicular distance from vehicle to route 
(y), yaw rate (r), forward speed (u), settling time (t^) and vehicle lookahead distance (Xj). 
A modified version of this simulation program is presented in Appendix B. The modified 
version is used to control the vehicle position in a general ( X-Y ) inertial system. The 
desired vehicle path is discretized into a series of straight line segments and the same 
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lookahead distance is used to regulate the vehicle deviation of each segment. Details 
of this modification are presented in the next paragraph. 

3. Graphics 

The GRAFSTAT graphic package is used to produce 2 dimensional graphs 
by using data from the simulation programs. 



B. DETAILS 

1. Poles and gains 

Calculate the system poles and gains for a given forward speed u and various 
combinations of t^ and Xj. Select t^ and Xj such that appropriate (sufficiently negative) 
poles and gains for the system are produced. This is verified by repeated simulations from 
step 2 that follows 

2. Distance in x-y axis 

Using the values of t^ and Xj from the previous step, the system response can 
be simulated. Unlike the control law design, the simulation is based on the full nonlinear 
equations of motion for the vehicle, (3), (4), (5) and (7). Typically, the initial conditions 
consist of nonzero values of the lateral deviation y and heading y. 

3. Distance in X-Y axis 

A similar procedure is used to simulate the vehicle response in a general path 
in the X-Y plane, as shown in Figure 2. The perpendicular distance y from the vehicle 
to the desired route in the X-Y plane is used to compute the commanded heading angle 
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Yj. The difference where a is the angle of the route with respect to the X-axis, is 
used instead of the heading \|/ in the control law. The above computations are performed 
by appropriate coordinate rotations between the two coordinate systems. 

When transiting from one straight line path to the next, the same lookahead distance 
Xj is used for both segments. The vehicle switches to the next segment when it gets 
within a specified distance from the terminal way point. This distance is measured along 
the x-axis and for given t^ and x^, and it should increase as the angle a for the next 
segment increases. Too high or too low values of this turning distance result in path 
overshoot and imdesirable oscillatory response. The optimum turning distance that allows 
for the smooth transition between consecutive straight line paths is established with the 
aid of the simulation program from step 2 as follows. 

For a fixed initial heading \\f, the initial deviation y is varied until the vehicle 
response is smooth and sufficiently fast with no path overshoot. The process is repeated 
for different initial conditions in \|i and a curve in y versus is constmcted. This is 
shown in Figure 3 and is the desirable turning distance versus turning angle curve. The 
actual curve is approximated by two straight lines which are used to initiate the turn in 
the simulation program. A copy of this simulation program is included in Appendix B. 
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X 







Figure 2 Angles and axes 
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TURNING DISTANCE VERSUS TURNING ANGLE 




Figure 3 Turning distance and turning angle 
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IV. SIMULATION RESULTS 



The vehicle parameters used in the simulation are: 



X 

u 


= -0.0076 




= 


0.0012 




X 

rr 


= 0.0040 




= 


-0.0550 






= 0.0200 


3’r 


= 


0.0300 






= -0.0010 


Vv 


= 


-0.1000 






= 0.0530 


>-8 


= 


0.0270 




^*.■8 


= 0.00173 


^Dy 


= 


0.3500 




•^88 


= -0.1000 


W 


= 


12000 


lb. 


n 

r 


= -0.0034 


1 


= 


17.4 


ft- 


fl 

V 


= 0.0012 


P 


= 


1.94 


sluglft\ 


n 

r 


= -0.0160 


g 


= 


32.2 


fi./scc. 


n 

V 


= -0.0074 


I 

2 


= 


10000 


ft*- 


•h 


= -0.0130 


V 


= 


0.000847 


ft /see 



The simulation begins by setting \j/ = 5 degrees, Xj = 2 vehicle lengths, t^ = 5, r 
= 0, V = 0 ft. /sec, u = 5 ft./sec. and y = 1. When the vehicle moves to a distance x = 20 
then the simulation stops. The route of vehicle and the rudder angle (6) that vehicle used 
during simulation are shown in Figure 4. The heading angle (\jr) and commanded heading 
(y,.) are shown in Figure 5. Yaw velocity and sway velocity are shown in Figure 6. 
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The values for and Xj were selected based on the results of the previous chapter. 
From the figures it can be seen that the vehicle response is very fast with limited 
overshoot. This, of course, depends heavily on the initial conditions of the simulation. The 
actual heading angle converges rather rapidly, after the initial transients have died out, to 
the commanded heading angle. 

The second series of simulations was performed in order to assess the capability 
of the control and guidance law to change course and keep the new path. Simulation 
parameters were Xj = 2, t^ = 5 and u = 5 as before. Initial conditions for the simulations 
were \|/ = 0, r = 0 and v = 0 with the initial vehicle position at (Xo,Yo) = (5,0) in the 
global reference frame. The first straight line segment is determined by the way points 
(5,0) and (25,0) and the second by (25,0) and (67.89,20). For this route the corresponding 
course change is 25 degrees. The results of this simulation are presented in Figure 7 
where along with the actual vehicle path, a side path at distance of 1 vehicle length off 
the desired path is shown. This corresponds to an arbitrary " safety path band " for the 
vehicle. The turning distance was fixed at 2 vehicle lengths throughout the simulations. 
From Figure 7 it can be seen that the vehicle turns to the new course smoothly with no 
path overshoot. When the second route changes to (25,0) and (41.78,20) which 
corresponds to 50 degrees course change, the results of Figure 8 show that a path 
overshoot occurs although it is yet not serious enough according to the artificial safety 
criterion described above. However, when the second route changes to (25,0) and 
(30.36,20) which corresponds to 75 degrees course change, significant vehicle oscillatoiy 
response and side path overshoot occurs, as demonstrated in Figure 9. 



19 



The above simulations demonstrate the need for adjustable turning distance; 
although path accuracy is obtained regardless of the value of the turning distance, the 
transient response during course change is not always within some predetermined safety 
bounds. For this reason we employ the built-in turning distance versus turning angle 
relationship shown in Figure 3 and repeat the simulations for the aforementioned three 
course changes. The results are shown in Figures 10, 1 1 and 12, where it can be seen that 
the vehicle response is now satisfactory for both course keeping and course changing. 

Finally, the last simulation test was performed in order to establish the capabilities 
of the scheme to follow a general path in the horizontal plane. For demonstration 
purposes the path was assumed to consist of the way points (0,10), (5,10), (25,0), (35,20), 
(50,20), (70,10), (50,-10) and (30,-10). Straight line segments were assumed as the desired 
paths between consecutive way points. The same simulation parameters were used as in 
the previous mns. The results of this simulation are presented in Figures 13 for adjustable 
turning distance, and 14 for the fixed turning distance case. It can be clearly seen that 
when the turning distance is function of turning angle, the scheme achieves excellent path 
keeping characteristics with smooth course changes and minimal path overshoot. 
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Figure 4 Pursuit navigation 
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TIME (DIMENSIONLESS) 



Figure 5 Pursuit navigation 
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SWAY VELOCITY (FT./SEC.) YAW VELOCITY (RADIAN/SEC.) 
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Figure 6 Pursuit navigation 
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Figure 7 Path control 
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Y POSITION 



TURNING ANGLE 50 DEGREES 



TURNING DISTANCE CONSTANT 




Figure 8 Path control 
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TURNING ANGLE 75 DEGREES 



TURNING DISTANCE CONSTANT 




Figure 9 Path control 
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Y POSITION 



TURNING ANGLE 25 DEGREES 
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Figure 10 Path control 
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Y POSITION 



TURNING ANGLE 50 DEGREES 



TURNING DISTANCE FUNCTION OF TURNING ANGLE 




Figure 1 1 Path control 
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Y POSITION 
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Figure 12 Path control 
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V. SUMMARY AND CONCLUSIONS 



The principal conclusions of this work can be summarized as follows: 

1. Orientation control law can be used in order to provide accurate vehicle path keeping 
when combined with an appropriate guidance scheme. 

2. Pure pursuit guidance was found to work very well for the autonomous underwater 
vehicle case and its simplicity make it a very attractive alternative to cross track error 
schemes. 

3. A built-in turning distance versus course change relationship can be utilized to initiate 
the turn at the appropriate time in order to avoid path overshoot and achieve smooth path 
transitions. 

4. It is expected that the added robustness that heading schemes naturally enjoy wdl aid 
in maintaining stability in cases where incomplete and inaccurate vehicle dynamic 
descriptions are available. 

Some recommendations for further research are as follows: 

1 . Comparative studies must be performed with other orientation guidance schemes such 
as proportional navigation and also with velocity guidance laws in order to ensure that 
the best technique is ultimately employed. 
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2. Similar studies must be performed in the vertical plane. Combined with the horizontal 
plane techniques developed in this work and with propulsion control they could be 
utilized to provide accurate trajectory following in 3-D space. 



33 



LIST OF REFERENCES 



1. Kanayama, Y. and Hartman, B.I. (1989) " Smooth local path planning for 
autonomous vehicles, " Proceeding, IEEE International Conference on Robotics and 
Automation, Scottsdale, Arizona. 

2. Lienard, D.L., (1990) " Autopilot design for autonomous underwater vehicles based 
on sliding mode control, " M.E. Thesis, Mechanical Engineering, Naval Postgraduate 
School, Monterey California. 

3. Chism, S., (1990) " Robust path tracking of autonomous underwater vehicles using 
sliding modes, " M.E. Thesis, Mechanical Engineering, Naval Postgraduate School, 
Monterey, California. 

4. Hawkinson, T., (1990) " Multiple input sliding mode control for autonomous diving 
and steering of underwater vehicles, " M.E. Thesis, Mechanical Engineering, Naval 
Postgraduate School, Monterey, California. 

5. Tan, C.H., (1986) " A simulation study of an autonomous steering system for on- 
road operation of automotive vehicles, " M.S. Thesis, Department of Computer Science, 
Naval Postgraduate School, Monterey, California. 

6. Smith. N.S., Crane, J.W., and Summey, D.C., (1978) " SDV simulator 
hydrodynamic coefficients, " Naval Coastal Systems Center, Panama City, Florida, Report 
No. NCSC-TM231-78. 



34 



APPENDIX A. 



1=17 .425; 
inquire xd_l 
inquire tc 
inquire u 
xd=xd_l * 1 ; 
aall=-0. 045380; 
aal2=-0. 351190; 
aa21=-0. 002795; 
aa22=-0 . 095680 ; 
bbl= 0.011432; 
bb2=-0 . 004273 ; 

OMEGA=(10.0*U)/(TC*L) ; 

AD1=1 . 75*OMEGA; 

AD2=2 . 15*OMEGA**2 ; 

AD3=OMEGA**3 ; 

Al=BBl*U*U; 

Bl=BB2*U*U; 

Cl=-ADl-( AA11+AA22 ) *U; 

A2=(BB1*AA22-BB2*AA12 ) *U**3; 
B2=(BB2*AA11-BB1*AA21 ) *U**3 ; 

Kl=AD3/( (BB2*AA11-BB1*AA21)*U**3) ; 
C2=AD2-(AA11*AA22-AA12*AA21 ) *U* * 2+BB2 *U*U* Kl 
K2=( Cl*B2-C2*Bl )/( Al *B2-A2 *Bl ) ; 

K3=( C2*A1-C1*A2 )/( Al*B2-A2*Bl ) ; 

all=0; 

al2=0; 

al 3=1 ; 

al4=0; 

a21=bbl*u*u*kl; 

a22=aall*u+bbl*u*u*k2; 

a23=aal2*u+bbl*u*u*k3; 

a24=bbl*u*u*kl/xd; 

a31=bb2*u*u*kl ; 

a32=aa21*u+bb2*u*u*k2; 

a33=aa22*u+bb2*u*u*k3; 

a34=bb2*u*u*kl/xd ; 

a41=u ; 

a42=l ; 

a43=0; 

a44 = 0; 

a=(all,al2,al3,al4;a21,a22,a23,a24; 

a31,a32,a33,a34;a41,a42,a43,a44); 
eig( a ) 
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APPENDIX B. 



PROGRAM SUB. FOR 



C 

C 

C 

C 

C 

C 

C 

C 



AUV LINE-OF-SIGHT NAVIGATION AND CONTROL 
VARIABLE GAINS INTERNALLY COMPUTED 



PROUTTICHAI SUWANDEE 
NAVAL POSTGRADUATE SCHOOL 
MARCH 1991 



REAL L , MASS , NRDOT , NVDOT , NR , NV , NDR 
REAL IZ,NU,LLL,NSL,Kl,K2,K3 

DIMENSION X(9) ,HH(9) ,VECl(9) ,VEC2(9) , TT( 1000 ) , YY ( 6 , 1000 ) , 

* ALPHA ( 10) ,XZ(10) ,YZ( 10) 

LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER( XRR=4 . E-3 ,XUDOT=-7 . 6E-3 , XVR=2 . E-2 , XRDR=-1 . E-3 , 

1 XVV=5 . 3E-2 ,XVDR=1 . 7 E-3 , XDRDR=-1 .E-2 ) 

LATERAL HYDRODYNAMIC COEFFICIENTS 

PARAMETER ( YRDOT=l . 2E-3 , YVDOT=-5 . 5E-2 , YR-3 . E-2 , YV=-1 . E-1 , 

1 YDR=2 . 7E-2 ,CDY=3 . 5E-1 ) 

YAW HYDRODYNAMIC COEFFICIENTS 

PARAMETER(NRDOT=-3 . 4E-3 ,NVDOT=l . 2E-3 ,NR=-1 . 6E-2,NV — 7 . 4E-3 , 
1 NDR=-1.3E-2) 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 

PARAMETER(WEIGHT=12000 . ,XG = 0 . , IZ=10000 . , L=17 . 4 ,RHO=l . 94 , 

1 G=32.2,NU=8.47E-4) 

OPEN ( 10 , FILE= ' SUB . IN' , STATUS® 'OLD' ) 

OPEN ( 11 , FILE® 'SUB. OUT' , STATUS® ' OLD ' ) 

READ (10,*) TARGET 

READ (10,*) TSIM, DELTA, IPRMT 

READ (10,*) PSI,R 

READ (10,*) U 

READ (10,*) TC,VC 

NUMBER OF POSITION OF ROUTE AND POSITION OF VEHICLE 
READ (10,*) N,XO,YO 
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POSITION OF ROUTE IN X-Y AXIS 
DO 30 1=1, N 

READ (10,*) XZ(I),YZ(I) 

30 CONTINUE 

TARGET=TARGET*L 
TWOPI =8 . 0*ATAN( 1 . 0 ) 

PI =0.5*TWOPI 

DO 999 M=1,N-1 

PSI1=0 

YTURN=0 

XYTURN=0 

PSI=PSI*PI/180 . 0 

MOVE ORIGINE OF AXIS TO THE FIRST POINT OF THE ROUTE 

XF=XZ(M+1)-XZ(M) 

IF (XF.EQ.O) XF=0. 0000001 
YF=YZ(M+1 )-YZ(M) 

XO=XO-XZ(M) 

IF (XO.EQ.O) XO=0. 0000001 
YO=YO-YZ(M) 

ANGLE OF POSITION OF VEHICLE 



ALPHA0=ATAN(ABS( YO/XO) ) 

IF ( (YO.GT.O ) .AND. (XO.LT.O) ) 
IF ( ( YO.LE.O) .AND. (XO.GT.O) ) 
IF ( (YO.LE.O ) .AND. (XO.LT.O) ) 

ANGLE OF ROUTE 

ALPHAl=ATAN(ABS( YF/XF) ) 

IF ( (YF.GT.O) .AND. (XF.LT.O) ) 
IF ( ( YF.LE. 0 ) .AND. (XF.GT. 0 ) ) 
IF ( (YF.LE. 0) .AND. (XF.LT.O) ) 



ALPHA0=PI-ALPHA0 

ALPHA0=2*PI-ALPHA0 

ALPHA0=PI+ALPHA0 



ALPHA1=PI-ALPHA1 

ALPHA1=2*PI-ALPHA1 

ALPHAl=PI+ALPHAl 



ANGLE BETWEEN VEHICLE AND ROUTE 
BETA= ( ALPHAO-ALPHAl ) 

DISTANCE FROM ORIGINE TO VEHICLE 
RT=(XO**2+YO**2 ) **0 . 5 

PROJECTED DISTANCE FROM ORIGINE TO VEHICLE ON THE ROUTE 



XS=RT*COS ( BETA) 

XC= ( XS ) *COS ( ALPHAl ) 
YC=( XS ) *SIN( ALPHAl ) 
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PERPENDICULAR DISTANCE OF VEHICLE TO X-AXIS 

YPOS=RT*SIN( BETA) 

TOTAL DISTANCE ON THE ROUTE 

TXD=(XF**2+YF**2 ) **0.5 
XD=ABS( TXD-XS ) 

HEADING ANGLE 

IF(M.EQ.l) PSI=PSI-ALPHA1 
IF (PSI.GT.PI) PSI=PSI-2*PI 
IF (PSI.LT.-PI) PSI=PSI+2*PI 
IF (M.EQ.N-1 ) GO TO 6 5 

NEXT HEADING ANGLE 

DUMY=YZ(M+2)-YZ(M+l ) 

DUMX=XZ(M+2)-XZ(M+l ) 

IF (DUMX.EQ.O) DUMX=0 . 0000001 
ALP2=ATAN(ABS(DUMY/DUMX) ) 

IF ( (DUMY.GT.O) .AND. (DUMX.LT.O) ) ALP2=PI-ALP2 
IF ( (DUMY.LE.O) .AND. (DUMX.GT.O) ) ALP2 = .''*PI-ALP2 
IF ( (DUMY.LE.O) .AND. (DUMX.LT.O) ) ALP2=I I+ALP2 
PSI1=ALPHA1-ALP2 
IF (PSIl.GT.PI) PSI1=PSI1-2*PI 
IF (PSIl.LT.-PI ) PSI1 = PSI1 + 2*PI 
PSI1=PSI1*180/PI 

TURNING DISTANCE 

IF(ABS(PSI1) .LE.45) YTURN=ABS(PSI1/25) 

IF( ABS( PSIl ) .GT. 45 ) YTURN= ( ABS ( PS 1 1 ) - 4 5 ) * 0 . 3/5 + 1 . 8 
XYTURN=YTURN/ABS( SIN( PSI1*PI/180 ) )-0 . 2 
IF(PSIl.EQ.O) XYTURN=0.0 

65 UC =U 

OMEGA=( 10 .0*U)/(TC*L) 

ADI =1.75*OMEGA 
AD2 =2 . 15*OMEGA**2 

AD3 =OMEGA**3 
C 

PISIM =TSIM/DELTA 
ISIM =PISIM 
ECHO =1.0/DELTA 

lECHO =IPRNT*20 
YAW =0.0 

SWAY =0.0 
V =0.0 

DR =0.n 

R =R*PI/180.0 
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ISTART=1 
XPOS =0.0 
YPOS -YPOS*L 



define the length X AND HEIGHT HH TERMS FOR THE INTEGRATION 



C 



C 

C 



C 



C 



X(l) = -105.9/12. 
X(2) = -99.3/12. 

X(3) = -87.3/12. 

X(4) = -66.3/12. 

X(5) = 72.7/12. 

X(6) = 83.2/12. 

X(7) = 91.2/12. 

X(8) = 99.2/12. 

X(9) = 103.2/12. 



HH(1) 
HH( 2 ) 
HH( 3) 
HH( 4 ) 
HH( 5 ) 
HH( 6 ) 
HH(7) 
HH( 8 ) 
HH( 9 ) 



0 . 00 / 12 . 
8.24/12. 
19.76/12. 
29.36/12. 
31.85/12. 
27 . 84/12 . 
21.44/12. 
12 . 00 / 12 . 
0 . 00 / 12 . 



MASS = WEIGHT/G 



Pl=MASS-0 . 5*RH0*L**3*XUD0T 
P3=MASS-0 . 5*RH0*L**3*YVD0T 
P4=MASS*XG-0 . 5*RHO*L**4*YRDOT 
P5=IZ-0 . 5*RH0*L**5*NRD0T 
P6=MASS*XG-0 . 5*RHO*L**4*NVDOT 
D =P5*P3-P4*P6 



AA11=( P5*0 . 5*RHO*L*L*YV-P4*0 . 5*RHO*L**3*NV)/D 
AA12=( P5* ( -MASS + 0 . 5 *RHO* L* * 3 * YR ) -P4 * ( -MASS*XG 
1 +0 . 5*RHO*L**4*NR ) )/D 

AA21=( P3*0 . 5*RHO*L**3*NV-P6*0 . 5*RHO*L*L*YV)/D 
AA22=( P3* ( -MASS*XG+0 . 5*RHO*L**4*NR)-P6* ( -MASS 
1 +0 . 5*RHO*L**3*YR) )/D 

BBl=( P5*0 . 5*RHO*L**2*YDR-P4*0. 5*RHO*L**3*NDR)/D 
BB2=( P3*0 . 5*RHO*L**3*NDR-P6*0. 5*RHO*L**2*YDR)/D 

Al =BBi*U*U 
Bl =BB2*U*U 

Cl =-ADl-(AAll+AA22 ) *U 
A2 =(-AA12*BB2+AA22*BB1 )*U**3 
B2 =(-AA21*BB1+AA11*BB2)*U**3 
Kl=AD3/( ( BB2*AAll-BBl*AA21 ) *U**3 ) 

C2 =AD2-( -AA12*AA21+AA11*AA22 ) *U* * 2+BB2 *U*U*K1 
K2=(C1*B2-C2*B1 ) /( Al *B2-A2 *Bl ) 

K3=( C2*A1-C1*A2 )/( Al*B2-A2*Bl ) 
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c 



600 



C 

C 



J =0 
IJ=0 
JE=0 
OFF=0 



DRHAT=0 . 0 
DRBAR=0 . 0 

SIMULATION BEGINS 

DO 100 I=1,ISIM 

CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 

DO 600 K=l,9 
UCF=V+X( K ) *R 
SGN=1 . 0 

IF (UCF.LT.0.0) SGN=-1.0 
VECl ( K)=HH( K) *UCF*UCF*SGN 
VEC2 ( K )=HH( K ) * UCF *UCF * SGN * X ( K ) 

CONTINUE 

CALL TRAP(9, VECl, X, SWAY) 

CALL TRAP(9,VEC2,X,YAW) 

SWAY^ — 0 . 5*RHO*CDY*SWAY 
YAW =-0 . 5*RHO*CDY*YAW 

FORCE EQUATIONS 

FP2 = -MASS*U*R+0 . 5*RHO*L**3*YR*U*R+0 . 5*RHO*L*L* ( 
YV*U*V+YDR*U*U*DR)+SWAY 

FP3 = -MASS*XG*U*R+0 . 5*RHO*L**4 *NR*U*R+0 . 5*RHO*L* * 3* 

( NV*U*V+NDR*U*U*DR ) +YAW 

VDOT =( p5*FP2-P4*FP3 )/( P5*P3-P4*P6) 

RDOT =( P6*FP2-P3*FP3 )/(P4*P6-P3*P5) 

PSIDOT=R 

YDOT =U*SIN( PSI )+V*COS( PSI )+VC 
XDOT =U*COS( PSI )-V*SIN( PSI ) 

FIRST ORDER INTEGRATION 

PSI =PSI +DELTA*PSIDOT 
V =V +DELTA*VDOT 
R =R +DELTA*RDOT 
XPOS=XPOS+DELTA*XDOT 
YPOS=YPOS+DELTA* YDOT 

YCTE=YPOS 
XAWAY=( XPOS-XD*L ) 

IF ( (XAWAY) .GE.-(XYTURN*L) ) OFF=l 
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RUDDER INPUT CALCULATION 
YA=ABS( YPOS ) 

HDM=ATAN( ( YPOS )/( -TARGET ) ) 

DR=K1*( PSI-HDM)+K2*V+K3*R 

IF (DR.GT. 0.4) DR= 0.4 
IF (DR.LE.-0.4) DR=-0.4 

PRINT RESULTS 

JE=JE+1 

IF ( JE.NE. lECHO) GO TO 99 
WRITE (*,*) ' XAWAY XAWAY/L 

JE=0 

99 J=J+1 

IF (J.NE.IPRNT) GO TO 100 

I J=I J+1 

TIME=I*DELTA 

XP=XPOS/L 

YP=YPOS/L 

XI=XZ (M)+XC+XP*COS( -ALPHAl )+YP*SIN( -ALPHAl ) 
YI=YZ(M)+YC+YP*C0S(-ALPHA1 ) -XP* S IN ( -ALPHAl ) 

WRITE (11,*) XI, YI 
0 = 0 

IF (OFF.EQ.l) GO TO 500 
100 CONTINUE 

500 PSI=PSI1 

XO=XI 
YO=YI 

999 CONTINUE 

STOP 
END 

SUBROUTINE TRAP ( N , A , B , OUT ) 

NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 

DIMENSION A( 1 ) ,B( 1 ) 

N1=N-1 
OUT=0 . 0 
DO 1 1=1, N1 

OUT1=0 .5*(A(I)+A(I+1))*(B(I+1)-B(D) 

OUT =OUT+OUTl 
1 CONTINUE 
RETURN 
END 
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